// LAMath - A SIMD-based Linear Algebra library
//
// Copyright (C) 2010  William K Killian
//
//  This program is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
//  The creator of this project can be contacted at william.killian@gmail.com
//

#ifndef LAMATH_HPP
#define LAMATH_HPP

#include <cmath>

#include <mmintrin.h>
#include <emmintrin.h>
#include <pmmintrin.h>
#include <xmmintrin.h>

#ifdef USE_SSE4
#include <nmmintrin.h>
#endif

#include <ostream>

namespace lamath
{

    float
    dot (const __m128 &x1, const __m128 &x2)
    {
	__m128 res;
#ifdef __SSE4_1__
	res = _mm_dp_ps (x1, x2, 0xFF);
#else
	res = _mm_mul_ps (x1, x2);
	res = _mm_hadd_ps (res, res);
	res = _mm_hadd_ps (res, res);
#endif
	return _mm_cvtss_f32 (res);
    }
  
    vector::vector ()
    {

    }
    
    vector::vector (float default_value)
    {
	m = _mm_set1_ps (default_value);
    }
    
    vector::vector (float x, float y, float z, float w)
    {
	m = _mm_setr_ps (x, y, z, w);
    }

    vector::vector (__m128 x)
    {
	m = x;
    }
    
    inline
    float&
    vector::operator()(size_t index)
    {
	return array [index];
    }
    
    inline
    float
    vector::operator()(size_t index) const
    {
	return array [index];
    }
    
    inline
    vector
    vector::operator+() const
    {
	return *this;
    }
    
    inline
    vector&
    vector::operator+= (const vector &other)
    {
	m = _mm_add_ps (m, other.m);
	return (*this);
    }
    
    inline
    vector&
    vector::operator-= (const vector &other)
    {
	m = _mm_sub_ps (m, other.m);
	return (*this);
    }
    
    inline
    vector
    vector::operator-() const
    {
	return vector (_mm_sub_ps (_mm_setzero_ps (), m));
    }

    inline
    bool
    vector::operator== (const vector &other) const
    {
	return _mm_movemask_ps (_mm_cmpneq_ps (m, other.m)) == 0;
    }
    
    inline
    bool
    vector::operator!= (const vector &other) const
    {
	return !operator==(other);
    }
    
    inline
    float
    vector::length () const
    {
	return sqrt (length_squared ());
    }
    
    inline
    float
    vector::length_squared () const
    {
	return dot (*this, *this);
    }
    
    inline
    void
    vector::normalize ()
    {
	_mm_mul_ps (m, _mm_rsqrt_ps (_mm_set1_ps (dot (*this, *this))));
    }

    inline
    vector
    vector::multiply (const vector &v, float s)
    {
	return vector (_mm_mul_ps (v.m, _mm_set1_ps (s)));
    }
    
    inline
    vector
    vector::divide (const vector &v, float s)
    {
  	return vector (_mm_div_ps (v.m, _mm_set1_ps (s)));
    }
    
    inline
    vector
    vector::add (const vector &v1, const vector &v2)
    {
	return vector (_mm_add_ps (v1.m, v2.m));
    }
    
    inline
    vector
    vector::subtract (const vector &v1, const vector &v2)
    {
	return vector (_mm_sub_ps (v1.m, v2.m));
    }
    
    inline
    float
    vector::dot (const vector &v1, const vector &v2)
    {
	__m128 x1 = v1.m;
	__m128 x2 = v2.m;
	__m128 res;
#ifdef __SSE4_1__
	res = _mm_dp_ps (x1, x2, 0xFF);
#else
	res = _mm_mul_ps (x1, x2);
	res = _mm_hadd_ps (res, res);
	res = _mm_hadd_ps (res, res);
#endif
	return _mm_cvtss_f32 (res);
    }
    
    inline
    vector
    vector::cross (const vector &v1, const vector &v2)
    {
	__m128 a_yzx = _mm_shuffle_ps (v1.m, v1.m, 0xC9); // right rotate
	__m128 b_yzx = _mm_shuffle_ps (v2.m, v2.m, 0xC9);
	__m128 c_zxy = _mm_sub_ps (_mm_mul_ps (v1.m, b_yzx), _mm_mul_ps (a_yzx, v2.m));
	return vector (_mm_shuffle_ps (c_zxy, c_zxy, 0xC9));
    }
    
    inline
    float
    vector::length (const vector &v)
    {
	return v.length ();
    }
    
    inline
    float
    vector::length_squared (const vector &v)
    {
	return v.length_squared ();
    }
    
    inline
    vector
    vector::normalize (const vector &v)
    {
	vector n (v);
	n.normalize ();
	return n;
    }
    
    matrix::matrix ()
    {
	
    }
    
    matrix::matrix (float default_val)
    {
	m [0] = _mm_set1_ps (default_val);
	m [1] = _mm_set1_ps (default_val);
	m [2] = _mm_set1_ps (default_val);
	m [3] = _mm_set1_ps (default_val);
    }
    
    matrix::matrix (const matrix &other)
    {
	m [0] = other.m [0];
	m [1] = other.m [1];
	m [2] = other.m [2];
	m [3] = other.m [3];
    }
    
    matrix::matrix (const __m128 x[])
    {
	m [0] = x [0];
	m [1] = x [1];
	m [2] = x [2];
	m [3] = x [3];
    }
    
    matrix::matrix (const __m128 r0, const __m128 r1, const __m128 r2, const __m128 r3)
    {
	m [0] = r0;
	m [1] = r1;
	m [2] = r2;
	m [3] = r3;
    }
    
    matrix::matrix (float m00, float m01, float m02, float m03, 
		    float m10, float m11, float m12, float m13, 
		    float m20, float m21, float m22, float m23, 
		    float m30, float m31, float m32, float m33)
    {
	m [0] = _mm_set_ps (m03, m02, m01, m00);
	m [1] = _mm_set_ps (m13, m12, m11, m10);
	m [2] = _mm_set_ps (m23, m22, m21, m20);
	m [3] = _mm_set_ps (m33, m32, m31, m30);
    }
    
    inline
    matrix&
    matrix::operator= (const matrix &other)
    {
	if (&other != this)
	{
	    m [0] = other.m [0];
	    m [1] = other.m [1];
	    m [2] = other.m [2];
	    m [3] = other.m [3];
	}
	return (*this);
    }
	
    inline
    float&
    matrix::operator() (size_t row, size_t col)
    {
	return array [row][col]; 
    }

    inline
    float
    matrix::operator() (size_t row, size_t col) const
    {
	return array [row][col]; 
    }
    
    inline
    vector
    matrix::row (size_t row) const
    {
	return vector (m [row]);
    }
	
    inline
    vector
    matrix::column (size_t col) const
    {
	__m128 c01, c23;

	if (col & 1)
	{
	    if ((col >> 1) & 1)
	    {
		// index 0b11
		c01 = _mm_shuffle_ps (m [1], m [0], 0xFF);
		c23 = _mm_shuffle_ps (m [3], m [2], 0xFF);
	    }
	    else
	    {
		// index 0b01
		c01 = _mm_shuffle_ps (m [1], m [0], 0x55);
		c23 = _mm_shuffle_ps (m [3], m [2], 0x55);
	    }
	}
	else
	{
	    if ((col >> 1) & 1)
	    {
		// index 0b10
		c01 = _mm_shuffle_ps (m [1], m [0], 0xAA);
		c23 = _mm_shuffle_ps (m [3], m [2], 0xAA);
	    }
	    else
	    {
		// index 0b00
		c01 = _mm_shuffle_ps (m [1], m [0], 0x00);
		c23 = _mm_shuffle_ps (m [3], m [2], 0x00);
	    }
	}
	return vector (_mm_shuffle_ps (c01, c23, 0x33));
    }

    inline
    matrix
    matrix::operator- () const
    {
	matrix mat (*this);
	__m128 zero = _mm_setzero_ps ();
	mat.m [0] = _mm_sub_ps (zero, m [0]);
	mat.m [1] = _mm_sub_ps (zero, m [1]);
	mat.m [2] = _mm_sub_ps (zero, m [2]);
	mat.m [3] = _mm_sub_ps (zero, m [3]);
	return (mat);
    }

    inline
    matrix
    matrix::operator+ () const
    {
	return (*this);
    }
    
    inline
    matrix&
    matrix::operator += (const matrix &other)
    {
	m [0] = _mm_add_ps (m [0], other.m [0]);
	m [1] = _mm_add_ps (m [1], other.m [1]);
	m [2] = _mm_add_ps (m [2], other.m [2]);
	m [3] = _mm_add_ps (m [3], other.m [3]);
	return (*this);
    }

    inline
    matrix&
    matrix::operator -= (const matrix &other)
    {
	m [0] = _mm_sub_ps (m [0], other.m [0]);
	m [1] = _mm_sub_ps (m [1], other.m [1]);
	m [2] = _mm_sub_ps (m [2], other.m [2]);
	m [3] = _mm_sub_ps (m [3], other.m [3]);
	return (*this);
    }
    
    inline
    bool
    matrix::operator== (const matrix &other) const
    {
	return (_mm_movemask_ps (_mm_cmpneq_ps (m [0], other.m [0])) == 0 &&
		_mm_movemask_ps (_mm_cmpneq_ps (m [1], other.m [1])) == 0 &&
		_mm_movemask_ps (_mm_cmpneq_ps (m [2], other.m [2])) == 0 &&
		_mm_movemask_ps (_mm_cmpneq_ps (m [3], other.m [3])) == 0);
    }
	
    inline
    bool
    matrix::operator!= (const matrix &m) const
    {
	return !operator== (m);
    }
	
    inline
    matrix
    matrix::inverse () const
    {
        // Hexidecimal Values
	//
        // 0x00 - 0 (load all index 0)
	// 0x55 - 1 (load all index 1)
	// 0xAA - 2 (load all index 2)
	// 0xFF - 3 (load all index 3)
	//
	// 0x80 - load a2, a0, b0, b0
	// 0x88 - load a2, a0, b2, b0
	// 0xA4 - load a2, a2, b1, b0
	//
	
        // Temporaries
	__m128 t0, t1, t2;
	
        // Initially Temporaries, then they will store Vectors, then inverse rows
	__m128 s0, s1, s2, s3;
	
	// Calculate Cofactors
	t0 = _mm_shuffle_ps (m[3], m[2], 0xFF);
	t1 = _mm_shuffle_ps (m[3], m[2], 0xAA);
	s0 = _mm_shuffle_ps (m[2], m[1], 0xAA);
	s1 = _mm_shuffle_ps (t0, t0, 0x80);
	s2 = _mm_shuffle_ps (t1, t1, 0x80);
	s3 = _mm_shuffle_ps (m[2], m[1], 0xFF);
	__m128 c0 = _mm_sub_ps (_mm_mul_ps (s0, s1), _mm_mul_ps (s2, s3));

	t1 = _mm_shuffle_ps (m[3], m[2], 0x55);
	s0 = _mm_shuffle_ps (m[2], m[1], 0x55);
	s2 = _mm_shuffle_ps (t1, t1, 0x80);
	__m128 c1 = _mm_sub_ps (_mm_mul_ps (s0, s1), _mm_mul_ps (s2, s3));

	t0 = _mm_shuffle_ps (m[3], m[2], 0xAA);
	s1 = _mm_shuffle_ps (t0, t0, 0x80);
	s3 = _mm_shuffle_ps (m[2], m[1], 0xAA);
	__m128 c2 = _mm_sub_ps (_mm_mul_ps (s0, s1), _mm_mul_ps (s2, s3));

	t1 = _mm_shuffle_ps (m[3], m[2], 0x00);
	s0 = _mm_shuffle_ps (m[2], m[1], 0x00);
	s2 = _mm_shuffle_ps (t1, t1, 0x80);
	__m128 c4 = _mm_sub_ps (_mm_mul_ps (s0, s1), _mm_mul_ps (s2, s3));

	t0 = _mm_shuffle_ps (m[3], m[2], 0xFF);
	s1 = _mm_shuffle_ps (t0, t0, 0x80);
	s3 = _mm_shuffle_ps (m[2], m[1], 0xFF);
	__m128 c3 = _mm_sub_ps (_mm_mul_ps (s0, s1), _mm_mul_ps (s2, s3));

	t0 = _mm_shuffle_ps (m[3], m[2], 0x55);
	s1 = _mm_shuffle_ps (t0, t0, 0x80);
	s3 = _mm_shuffle_ps (m[2], m[1], 0x55);
	__m128 c5 = _mm_sub_ps (_mm_mul_ps (s0, s1), _mm_mul_ps (s2, s3));
	
	// Signs
	
	__m128 pos = _mm_set_ps ( 1.0f,-1.0f, 1.0f,-1.0f);
	__m128 neg = _mm_set_ps (-1.0f, 1.0f,-1.0f, 1.0f);
	
	// Calculate Vectors
	
	t0 = _mm_shuffle_ps (m[1], m[0], 0x00);
	s0 = _mm_shuffle_ps (t0, t0, 0xA4);
	t0 = _mm_shuffle_ps (m[1], m[0], 0x55);
	s1 = _mm_shuffle_ps (t0, t0, 0xA4);
	t0 = _mm_shuffle_ps (m[1], m[0], 0xAA);
	s2 = _mm_shuffle_ps (t0, t0, 0xA4);
	t0 = _mm_shuffle_ps (m[1], m[0], 0xFF);
	s3 = _mm_shuffle_ps (t0, t0, 0xA4);

	// Calculate Columns
	
	t0 = _mm_mul_ps (s1, c0);
	t1 = _mm_mul_ps (s2, c1);
	t2 = _mm_mul_ps (s3, c2);
	__m128 i0 = _mm_mul_ps (neg, _mm_add_ps (t2, _mm_sub_ps (t0, t1)));

	t0 = _mm_mul_ps (s0, c0);
	t1 = _mm_mul_ps (s2, c3);
	t2 = _mm_mul_ps (s3, c4);
	__m128 i1 = _mm_mul_ps (pos, _mm_add_ps (t2, _mm_sub_ps (t0, t1)));

	t0 = _mm_mul_ps (s0, c1);
	t1 = _mm_mul_ps (s1, c3);
	t2 = _mm_mul_ps (s3, c5);
	__m128 i2 = _mm_mul_ps (neg, _mm_add_ps (t2, _mm_sub_ps (t0, t1)));

	t0 = _mm_mul_ps (s0, c2);
	t1 = _mm_mul_ps (s1, c4);
	t2 = _mm_mul_ps (s2, c5);
	__m128 i3 = _mm_mul_ps (pos, _mm_add_ps (t2, _mm_sub_ps (t0, t1)));
	
	// Get First Row of Inverse
	
	s0 = _mm_shuffle_ps (i0, i1, 0x00);
	s1 = _mm_shuffle_ps (i2, i3, 0x00);
	s2 = _mm_shuffle_ps (s0, s1, 0x88);
	
	// Calculate Determinant
	
	__m128 d = _mm_mul_ps (m [0], s2);
	d = _mm_hadd_ps (d, d);
	d = _mm_hadd_ps (d, d);
	d = _mm_rcp_ps (d);
	
	return matrix (_mm_mul_ps (i0, d),
		       _mm_mul_ps (i1, d),
		       _mm_mul_ps (i2, d),
		       _mm_mul_ps (i3, d));
    }
    
    inline
    matrix
    matrix::transpose () const
    {
	__m128 r0 = m [0];
	__m128 r1 = m [1];
	__m128 r2 = m [2];
	__m128 r3 = m [3];
	// built-in macro for transpose
	_MM_TRANSPOSE4_PS (r0, r1, r2, r3);
	return matrix (r0, r1, r2, r3);
    }

    inline
    matrix
    matrix::multiply (const matrix &m, float s)
    {
	__m128 mul = _mm_set1_ps (s);
	matrix mat (m);
	mat.m [0] = _mm_mul_ps (m.m [0], mul);
	mat.m [1] = _mm_mul_ps (m.m [1], mul);
	mat.m [2] = _mm_mul_ps (m.m [2], mul);
	mat.m [3] = _mm_mul_ps (m.m [3], mul);
	return (mat);
    }
    
    inline
    matrix
    matrix::divide (const matrix &m, float s)
    {
	__m128 div = _mm_set1_ps (s);
	matrix mat (m);
	mat.m [0] = _mm_div_ps (m.m [0], div);
	mat.m [1] = _mm_div_ps (m.m [1], div);
	mat.m [2] = _mm_div_ps (m.m [2], div);
	mat.m [3] = _mm_div_ps (m.m [3], div);
	return (mat);
    }
    
    inline
    matrix
    matrix::add (const matrix &m1, const matrix &m2)
    {
	matrix mat (m1);
	return (mat += m2);
    }
    
    inline
    matrix
    matrix::subtract (const matrix &m1, const matrix &m2)
    {
	matrix mat (m1);
	return (mat -= m2);
    }
	
    inline
    matrix
    matrix::multiply (const matrix &m1, const matrix &m2)
    {
	matrix t = m2.transpose ();
	return matrix (_mm_setr_ps (dot (m1.m [0], t.m [0]), dot (m1.m [0], t.m [1]), dot (m1.m [0], t.m [2]), dot (m1.m [0], t.m [3])),
		       _mm_setr_ps (dot (m1.m [1], t.m [0]), dot (m1.m [1], t.m [1]), dot (m1.m [1], t.m [2]), dot (m1.m [1], t.m [3])),
		       _mm_setr_ps (dot (m1.m [2], t.m [0]), dot (m1.m [2], t.m [1]), dot (m1.m [2], t.m [2]), dot (m1.m [2], t.m [3])),
		       _mm_setr_ps (dot (m1.m [3], t.m [0]), dot (m1.m [3], t.m [1]), dot (m1.m [3], t.m [2]), dot (m1.m [3], t.m [3])));
    }
    
    inline
    matrix
    matrix::inverse (const matrix &m)
    {
	return m.inverse ();
    }
	
    inline
    matrix
    matrix::transpose (const matrix &m)
    {
	return m.transpose ();
    }

}

inline
lamath::vector
operator* (const lamath::matrix &m, const lamath::vector &v)
{
    lamath::matrix mat = m.transpose ();
    return lamath::vector (lamath::vector::dot (mat.m [0], v.m),
			   lamath::vector::dot (mat.m [1], v.m),
			   lamath::vector::dot (mat.m [2], v.m),
			   lamath::vector::dot (mat.m [3], v.m));
}

inline
std::ostream&
operator<< (std::ostream& out, const lamath::vector &v)
{
    return (out << v.x << " "
	    << v.y << " "
	    << v.z << " "
	    << v.w);
}
  
inline
lamath::vector
operator+ (const lamath::vector &v1, const lamath::vector &v2)
{
    return lamath::vector::add (v1, v2);
}

inline     
lamath::vector
operator- (const lamath::vector &v1, const lamath::vector &v2)
{
    return lamath::vector::subtract (v1, v2);
}

inline
lamath::vector
operator* (const lamath::vector &v, float s)
{
    return lamath::vector::multiply (v, s);
}

inline
lamath::vector
operator* (float s, const lamath::vector &v)
{
    return operator* (v, s);
}

inline
lamath::vector
operator/ (const lamath::vector &v, float s)
{
    return lamath::vector::divide (v, s);
}

inline
std::ostream&
operator<< (std::ostream &out, const lamath::matrix &m)
{
    return (out << m.m00 << " " << m.m01 << " " << m.m02 << " " << m.m03 << "\n"
	    << m.m10 << " " << m.m11 << " " << m.m12 << " " << m.m13 << "\n"
	    << m.m20 << " " << m.m21 << " " << m.m22 << " " << m.m23 << "\n"
	    << m.m30 << " " << m.m31 << " " << m.m32 << " " << m.m33 << "\n");
}

inline
lamath::matrix
operator+ (const lamath::matrix &m1, const lamath::matrix &m2)
{
    return lamath::matrix::add (m1, m2);
}
   
inline   
lamath::matrix
operator- (const lamath::matrix &m1, const lamath::matrix &m2)
{
    return lamath::matrix::subtract (m1, m2);
}

inline
lamath::matrix
operator* (const lamath::matrix &m1, const lamath::matrix &m2)
{
    return lamath::matrix::multiply (m1, m2);
}

inline
lamath::matrix
operator* (const lamath::matrix &m, float s)
{
    return lamath::matrix::multiply (m, s);
}

inline
lamath::matrix
operator* (float s, const lamath::matrix &m)
{
    return operator* (m, s);    
}

inline
lamath::matrix
operator/ (const lamath::matrix &m, float s)
{
    return lamath::matrix::divide (m, s);
}

#endif
